#pragma once
#include "motion/rtcmds.h"
#include "zuc/nml_intf/zucpos.h"
#include "zuc/motion/motion.h"
#include "dyna/jaka_dyn_intf.h"

/*****************************************************************************
 * 运动相关的指令，每个指令都需要在相应的模式下才允许调用
******************************************************************************/

#define MOVING_CMD_CLASS_NAME(x) CMD_CLASS_NAME(x) "[MOVING CMD]"

class CmdJogStop : public mot::MultiRobCmd
{
public:
    CmdJogStop(int grp_id)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdJogStop);
    };
    ~CmdJogStop() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };
};

class CmdInFreeMode : public mot::MultiRobCmd
{
public:
    enum JOG_TYPE
    {
        JOG_WORLD = 0,
        JOG_JOINT = 1,
        JOG_TOOL = 2,
    };
    CmdInFreeMode() = default;
    virtual ~CmdInFreeMode() = default;

    virtual int active_callback();
    virtual int process();
    double vel_;
    int jog_mode_;
    int jog_axis_;
    double offset_;
};

class CmdJogCont : public CmdInFreeMode
{
public:
    CmdJogCont(int robid, int jog_mode, int axis_num, double vel)
    {
        group_id_ = robid;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdJogCont);
        jog_axis_ = axis_num;
        jog_mode_ = jog_mode;
        vel_ = vel;
    };
    ~CmdJogCont() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };
};

class CmdJogIncr : public CmdInFreeMode
{
public:
    CmdJogIncr(int grp_id, int jog_mode, int axis_num, double vel, double offset)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdJogIncr);
        jog_axis_ = axis_num;
        jog_mode_ = jog_mode;
        vel_ = vel;
        offset_ = offset;
    };
    ~CmdJogIncr() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };
};

class CmdJogAbs : public CmdInFreeMode
{
public:
    CmdJogAbs(int grp_id, int jog_mode, int axis_num, double vel, double offset)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdJogAbs);
        jog_axis_ = axis_num;
        jog_mode_ = jog_mode;
        vel_ = vel;
        offset_ = offset;
    };
    ~CmdJogAbs() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };
};

class CmdJogMultiJoint : public CmdInFreeMode
{
public:
    CmdJogMultiJoint(int grp_id, double vel, RobJointVal target)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdJogMultiJoint);
        vel_ = vel;
        target_ = target;
        jog_mode_ = JOG_JOINT;
        jog_axis_ = 0;  // 无用参数
    };
    ~CmdJogMultiJoint() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };

    RobJointVal target_;
};

class CmdInCoordMode : public mot::MultiRobCmd
{
public:
    CmdInCoordMode() = default;
    virtual ~CmdInCoordMode() = default;

    virtual int active_callback();
    virtual int process();
};

// 联动JOG
class CmdJogMovj : public CmdInCoordMode
{
public:
    CmdJogMovj(int grp_id, const zuc_jog_t& jog)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdJogMovj);
        jog_ = jog;
    };
    ~CmdJogMovj() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };

    zuc_jog_t jog_;
};

class CmdRigidTap : public mot::MultiRobCmd
{
public:
    CmdRigidTap(int grp_id, int id, const zuc_traj_rigid_tap_t& trajrigidtap)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdRigidTap);
        id_ = id;
        trajrigidtap_ = trajrigidtap;
    };
    ~CmdRigidTap() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int id_;
    zuc_traj_rigid_tap_t trajrigidtap_;
};

class CmdJogMovl : public CmdInCoordMode
{
public:
    enum JogMovlType
    {
        JOG_MOVL_ABS,
        JOG_MOVL_INCR,
        JOG_MOVL_TOOL_INCR,
    };
    CmdJogMovl(int grp_id, JogMovlType type, zuc_jog_t jog)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdJogMovl);
        type_ = type;
        jog_ = jog;
    };
    ~CmdJogMovl() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };

    JogMovlType type_;
    zuc_jog_t jog_;
};

class CmdJogMovc : public mot::MultiRobCmd
{
public:
    CmdJogMovc(int grp_id, int incr, const zuc_jog_t& jog)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdJogMovc);
        incr_ = incr;
        jog_ = jog;
    };
    ~CmdJogMovc() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int incr_;
    zuc_jog_t jog_;
};

class CmdSetLine : public mot::MultiRobCmd
{
public:
    CmdSetLine(int grp_id, int id, int motion_type, const zuc_traj_linearmov_t& trajlinearmov)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdSetLine);
        id_ = id;
        motion_type_ = motion_type;
        trajlinearmov_ = trajlinearmov;
    };
    ~CmdSetLine() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int id_;
    int motion_type_;
    zuc_traj_linearmov_t trajlinearmov_;
};

class CmdSetMovc : public mot::MultiRobCmd
{
public:
    CmdSetMovc(int grp_id, int id, int motion_type, zuc_traj_movc_t trajmovc)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdSetMovc);
        id_ = id;
        trajmovc_ = trajmovc;
        motion_type_ = motion_type;
    };
    ~CmdSetMovc() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int id_;
    int motion_type_;
    zuc_traj_movc_t trajmovc_;
};
class CmdSetMovs : public mot::MultiRobCmd
{
public:
    CmdSetMovs(int grp_id, int id, const zuc_traj_movs_t& trajmovs)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdSetMovs);
        id_ = id;
        trajmovs_ = trajmovs;
    };
    ~CmdSetMovs() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int id_;
    zuc_traj_movs_t trajmovs_;
};
class CmdSetCircle : public mot::MultiRobCmd
{
public:
    CmdSetCircle(int grp_id, int id, zuc_traj_circular_mov_t trajcircularmove)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdSetCircle);
        id_ = id;
        trajcircularmove_ = trajcircularmove;
    };
    ~CmdSetCircle() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int id_;
    zuc_traj_circular_mov_t trajcircularmove_;
};
class CmdSetJointLine : public mot::MultiRobCmd
{
public:
    CmdSetJointLine(int grp_id, int id, int motion_type, zuc_traj_jointmov_t trajjointmov)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdSetJointLine);
        id_ = id;
        motion_type_ = motion_type;
        trajjointmov_ = trajjointmov;
    };
    ~CmdSetJointLine() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int id_;
    int motion_type_;
    zuc_traj_jointmov_t trajjointmov_;
};

class CmdSetRobotUserId : public mot::MultiRobCmd
{
public:
    CmdSetRobotUserId(int grp_id, int id)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdSetRobotUserId);
        id_ = id;
    };
    ~CmdSetRobotUserId() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int id_;
};

class CmdSetRobotToolId : public mot::MultiRobCmd
{
public:
    CmdSetRobotToolId(int grp_id, int id)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdSetRobotToolId);
        id_ = id;
    };
    ~CmdSetRobotToolId() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };

    int id_;
};

class CmdPause : public mot::MultiRobCmd
{
public:
    CmdPause(int grp_id)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdPause);
    };
    ~CmdPause() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };
};

class CmdResume : public mot::MultiRobCmd
{
public:
    CmdResume(int grp_id)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdResume);
    };
    ~CmdResume() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };
};

class CmdRapidScale : public mot::MultiRobCmd
{
public:
    CmdRapidScale(int grp_id, double scale)
    {
        group_id_ = grp_id;
        cmd_name_ = MOVING_CMD_CLASS_NAME(CmdRapidScale);
        scale_ = scale;
    };
    ~CmdRapidScale() {}

    virtual int active_callback();
    virtual int process();
    virtual int check_para() { return 0; };

    double scale_;
};